#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <unistd.h>

using namespace std;

/* 在 SLAM 中,我们经常需要进行坐标变换,请用 Eigen 库实现坐标变换:
 * 将点从世界坐标系转换到机器人坐标系。从机器人坐标系到世界坐标系的旋转方向为,先沿
 * X 轴 45°旋转,再沿 Y 轴 30°旋转。
 * 
*/
int main()
{
    // 定义一个点在世界坐标系中的坐标P_w
    Eigen::Vector4d P_w(1.0, 2.0, 3.0, 1.0);

    // 定义机器人在世界坐标系中的位置t_wr
    Eigen::Vector3f t_wr(4.0, 5.0, 6.0);

    // --------- 开始你的代码	---------------//

    // 机器人坐标系到世界坐标系的旋转向量表示
    Eigen::AngleAxisd ro_vec1 (M_PI/4,Eigen::Vector3d(1,0,0));
    Eigen::AngleAxisd ro_vec2 (M_PI/6,Eigen::Vector3d(0,1,0));
    // cout << "旋转向量的旋转轴: \n" << ro_vec1.axis() << " \n旋转向量的角度: " << ro_vec1.angle() << endl;
    // cout << "旋转向量的旋转轴: \n" << ro_vec2.axis() << " \n旋转向量的角度: " << ro_vec2.angle() << endl;

    // 旋转向量转为旋转矩阵
    // 构建旋转矩阵R_wc
    Eigen::Matrix3d R_wc = ro_vec2.matrix() * ro_vec1.matrix();


    // 构建变换矩阵T_wc
    Eigen::Isometry3d T_wc = Eigen::Isometry3d::Identity();
    for (int i = 0;i < 3;i++)
    {
        for (int j = 0;j < 3;j++)
        {
            T_wc(i,j) = R_wc(i,j);
        }
    }
    cout << T_wc.matrix() << endl;


    // 计算点在机器人坐标系中的坐标P_c = T_cw * P_w
    // cout << T_wc.matrix().reverse() << endl;
    Eigen::Vector4d P_c = T_wc.matrix().reverse() * P_w;


    // --------- 结束你的代码	---------------//
    std::cout << "end " << std::endl;

    std::cout << "The point in the robot coordinate system is: \n" << P_c << std::endl;

    uint64_t cnt = 0;
    while(1)
    {
        cnt++;
        sleep(10);
        cout << "cnt: " << cnt << endl;
    }

    return 0;
}
